/**
 * This is a TOSSIM-specific implementation of SensorInterruptC that
 * directly emulates SensorInterrupt.
 *
 * by Mina Jung @SU (March 2010)
 ****/

#include <Interrupt.h>

module SensorInterruptC {
  provides interface Init;
  provides interface Interrupt as SensorInterrupt[uint8_t num];
}


implementation {


  enum {
    INTERRUPT_COUNT = uniqueCount(UQ_SENSOR_INTERRUPT)
  };

  typedef struct tossim_interrupt {
    bool isActive;
    sim_event_t* evt;
  } tossim_interrupt_t;

  tossim_interrupt_t interrupts[INTERRUPT_COUNT];

  sim_time_t initTime;

  void initializeEvent(sim_event_t* evt, uint8_t interruptID);
  
  sim_time_t clockToSim(uint32_t clockVal) {
    uint32_t temp;
    sim_time_t converted; 
    temp = (clockVal * sim_ticks_per_sec()) / 1024;
    converted.sec = temp/sim_ticks_per_sec();
    converted.ticks = temp%sim_ticks_per_sec();

    return (converted);
  }

  uint32_t simToClock(sim_time_t sim) {
    uint32_t temp;

    temp =  ( (sim.sec*sim_ticks_per_sec() + sim.ticks) * 1024 ) / sim_ticks_per_sec();
    return (temp);
  }
  
  command error_t Init.init() {
    memset(interrupts, 0, sizeof(interrupts));
    initTime = sim_time();
    return SUCCESS;
  }

  command void SensorInterrupt.startPeriodic[uint8_t id]( uint32_t dt ) {
    call SensorInterrupt.startPeriodicAt[id](call SensorInterrupt.getNow[id](), dt);
  }
  command void SensorInterrupt.startOneShot[uint8_t id]( uint32_t dt ) {
    call SensorInterrupt.startOneShotAt[id](call SensorInterrupt.getNow[id](), dt);
  }

  command void SensorInterrupt.stop[uint8_t id]() {
    interrupts[id].isActive = 0;
    if (interrupts[id].evt != NULL) {
      interrupts[id].evt->cancelled = 1;
      interrupts[id].evt->cleanup = sim_queue_cleanup_total;
      interrupts[id].evt = NULL;
    }
  }

  // extended interface
  command bool SensorInterrupt.isRunning[uint8_t id]() {return interrupts[id].isActive;}
  command bool SensorInterrupt.isOneShot[uint8_t id]() {return !interrupts[id].isActive;}
  
  command void SensorInterrupt.startPeriodicAt[uint8_t id]( uint32_t t0, uint32_t dt ) {
    call SensorInterrupt.startOneShotAt[id](t0, dt);
    interrupts[id].isPeriodic = 1;
  }
  command void SensorInterrupt.startOneShotAt[uint8_t id]( uint32_t t0, uint32_t dt ) {
    uint32_t currentTime = call SensorInterrupt.getNow[id]();
    sim_time_t fireTime = sim_time();
    sim_time_t diffTime;

    call SensorInterrupt.stop[id]();
    
    interrupts[id].evt = sim_queue_allocate_event();
    initializeEvent(interrupts[id].evt, id);

    diffTime = clockToSim(dt);
    fireTime.sec += diffTime.sec;
    fireTime.ticks += diffTime.ticks;
    fireTime.sec += fireTime.ticks/sim_ticks_per_sec();
    fireTime.ticks = fireTime.ticks%sim_ticks_per_sec();
    
    // Be careful about signing and casts, etc.
    if (currentTime > t0) {
      diffTime = clockToSim(currentTime - t0);
      fireTime.sec -= diffTime.sec;
      fireTime.ticks -= diffTime.ticks;
      if(fireTime.ticks < 0){
        fireTime.sec -= 1;
        fireTime.ticks = sim_ticks_per_sec() + fireTime.ticks;
      }
    }
    else {
      diffTime = clockToSim(t0 - currentTime);
      fireTime.sec += diffTime.sec;
      fireTime.ticks += diffTime.ticks;
      fireTime.sec += fireTime.ticks/sim_ticks_per_sec();
      fireTime.ticks = fireTime.ticks%sim_ticks_per_sec();
    }

    interrupts[id].evt->time = fireTime;
    interrupts[id].isPeriodic = 0;
    interrupts[id].isActive = 1;
    interrupts[id].t0 = t0;
    interrupts[id].dt = dt;

    sim_queue_insert(interrupts[id].evt);
  }
    
  command uint32_t SensorInterrupt.getNow[uint8_t id]() {
    sim_time_t nowTime = sim_time();
    uint32_t nowMS;
   
    nowTime.sec -= initTime.sec;
    nowTime.ticks -= initTime.ticks;

    if(nowTime.ticks < 0){
      nowTime.sec -= 1;
      nowTime.ticks = sim_ticks_per_sec() + nowTime.ticks;
    }

    nowMS = simToClock(nowTime);

    return nowMS & 0xffffffff;
  }
  
  command uint32_t SensorInterrupt.gett0[uint8_t id]() {
    return interrupts[id].t0;
  }
  command uint32_t SensorInterrupt.getdt[uint8_t id]() {
    return interrupts[id].dt;
  }

  void tossim_interrupt_handle(sim_event_t* evt) {
    uint8_t* datum = (uint8_t*)evt->data;
    uint8_t id = *datum;
    signal SensorInterrupt.fired[id]();

    // We should only re-enqueue the event if it is a follow-up firing
    // of the same timer.  If the timer is stopped, it's a one shot,
    // or someone has started a new timer, don't re-enqueue it.
    if (interrupts[id].isActive &&
	interrupts[id].isPeriodic &&
	interrupts[id].evt == evt) {
      sim_time_t temp = clockToSim(interrupts[id].dt);
      evt->time.sec += temp.sec;
      evt->time.ticks += temp.ticks;
      evt->time.sec += evt->time.ticks/sim_ticks_per_sec();
      evt->time.ticks = evt->time.ticks%sim_ticks_per_sec();
      
      sim_queue_insert(evt);
    }
    // If we aren't enqueueing it, and nobody has done something that
    // would cause the event to have been garbage collected, then do
    // so.
    else if (interrupts[id].evt == evt) {
      call SensorInterrupt.stop[id]();
    }
  }
  
  void initializeEvent(sim_event_t* evt, uint8_t interruptID) {
    uint8_t* data = (uint8_t*)malloc(sizeof(uint8_t));
    *data = interruptID;

    evt->handle = tossim_interrupt_handle;
    evt->cleanup = sim_queue_cleanup_none;
    evt->data = data;
  }

 default event void SensorInterrupt.fired[uint8_t id]() {}
}

